//
//  atan2.c
//
//
//  Created by Bram Drenthe on 13-12-13.
//
//

#include <stdio.h>

#include "qmath.h"
#include <math.h>

static const q32_t FOUR_DIV_PI  = 0x145F3;            /*!< Fix16 value of 4/PI */
static const q32_t _FOUR_DIV_PI2 = 0xFFFF9840;        /*!< Fix16 value of -4/PI² */
static const q32_t X4_CORRECTION_COMPONENT = 0x399A;  /*!< Fix16 value of 0.225 */
static const q32_t PI_DIV_4 = 0x0000C90F;             /*!< Fix16 value of PI/4 */

static const q32_t THREE_PI_DIV_4 = 0x00025B2F;       /*!< Fix16 value of 3PI/4 */

q32_t qMul(q32_t a, q32_t b)
{
    long long product = (long long)a * b;
    
    if (product < 0) product--;
    q32_t result = product >> 16;
    result += (product & 0x8000) >> 15;
    return result;
}

q32_t qDiv(q32_t a, q32_t b)
{
    return (q32_t)( (((long long)a)<<(16))/b );
}

q32_t qSqrt(q32_t num)
{
    unsigned int result = 0;
    unsigned int bit;
    unsigned char n;
    
    // Many numbers will be less than 15, so
    // this gives a good balance between time spent
    // in if vs. time spent in the while loop
    // when searching for the starting value.
    if (num & 0xFFF00000)
        bit = (unsigned int)1 << 30;
    else
        bit = (unsigned int)1 << 18;
    
    while (bit > num) bit >>= 2;
    
    // The main part is executed twice, in order to avoid
    // using 64 bit values in computations.
    for (n = 0; n < 2; n++)
    {
        // First we get the top 24 bits of the answer.
        while (bit)
        {
            if (num >= result + bit)
            {
                num -= result + bit;
                result = (result >> 1) + bit;
            }
            else
            {
                result = (result >> 1);
            }
            bit >>= 2;
        }
        
        if (n == 0)
        {
            // Then process it again to get the lowest 8 bits.
            if (num > 65535)
            {
                // The remainder 'num' is too large to be shifted left
                // by 16, so we have to add 1 to result manually and
                // adjust 'num' accordingly.
                // num = a - (result + 0.5)^2
                //       = num + result^2 - (result + 0.5)^2
                //       = num - result - 0.5
                num -= result;
                num = (num << 16) - 0x8000;
                result = (result << 16) + 0x8000;
            }
            else
            {
                num <<= 16;
                result <<= 16;
            }
            
            bit = 1 << 14;
        }
    }
    
    // Finally, if next bit would have been 1, round the result upwards.
    if (num > result)
    {
        result++;
    }
    
    return result;
}

int main()
{
    float f;
    for (f=0;f<1000;f+=0.1f)
    {
        q32_t a = qFromFloat(f);
        float real = sqrt(f);
        q32_t b = qSqrt(a);
        float ee = floatFromQ(b);
        printf("%.16f;%.16f;%.16f;%.16f\n",f,real,ee,real-ee);
    }
}